package clubhouse.motioncommand;

import java.awt.geom.Point2D;
import java.util.Vector;

import clubhouse.base.Landmarks;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class MotionPlan {

	/**Current value of distance info for the next move*/
	private int pulse_dist;
	/**Current value of rotation info for the next move*/
	private int pulse_rot;
	/**Current value of rotation direction for the next move, 1 if trigonometric direction, 0 otherwise.*/
	private int trigo;
	/**Only used in scenario 1, node id of current destination.*/
	private int index_current_dest;
	/**Current scenario.*/
	private int scenario;
	/**Coordinates of current destination [X coordinate, Y coordinate]*/
	private double[] destinationCoord;
	
	/**DecisionMaker instance, used to find the next destination according to robot position, scenario and algorithm.*/
	private DecisionMaker decider;
	/**CalculInfo instance, used to get the move information to go to the next destination.*/
	private CalculInfo calculator;
	
	/**
	 * Constructs the MotionPlan object. Initialize move information, store current scenario and instantiate
	 * DecisionMaker and CalculInfo objects.
	 * @param scenario the value of current simulation's scenario.
	 */
	public MotionPlan(int scenario) {		
		this.pulse_dist = 0;
		this.pulse_rot = 0;
		this.trigo = 0;
		this.index_current_dest = 1;	//In scenario 1, the first destination is node 1
		this.decider = new DecisionMaker(scenario);
		this.calculator = new CalculInfo();
		this.destinationCoord = new double[2];
			
		this.scenario = scenario;

	}
	
	/**
	 * Get the current Voronoi polygons.
	 * @return the vector of points array representing the set of current polygons, each points array represent
	 * the vertices set of one polygon.
	 */
	public Vector<Point2D[]> getPolygons(){
		return(this.decider.getPolygons());
	}
	
	/**
	 * Return the calculated distance information for the next move.
	 * @return the value, in pulses, of the distance information.
	 */
	public Integer getPulse_dist() {
		return this.pulse_dist;
	}

	/**
	 * Return the calculated rotation information for the next move.
	 * @return the value, in pulses, of the rotation move information.
	 */
	public Integer getPulse_rot() {
		return this.pulse_rot;
	}

	/**
	 * Return the calculated rotation direction information for the next move.
	 * @return the value of rotation direction, 1 if trigonometric direction, 0 otherwise.
	 */
	public Integer getTrigo() {
		return this.trigo;
	}
	
	/**
	 * Return the coordinates of the current destination.
	 * @return the double array containing the coordinates of current destination [X coordinate,Y coordinate]
	 */
	public double[] getDestinationCoord() {
		return this.destinationCoord;
	}

	/**
	 * Get the distance information for next move in units. Convert it from pulses to units.
	 * @return the value, in units, of the distance information.
	 */
	public double getDistance() {
		return (this.pulse_dist/CalculInfo.delta);
	}

	/**
	 * Get the rotation information for next move in rad. Convert it from pulses to rad.
	 * @return the value, in rad, of the rotation information.
	 */
	public double getRotation() {
		return (this.pulse_rot/CalculInfo.omega);
	}
	
	/**
	 * Decide on the next destination then calculate the information (distance & rotation pulses and rotation 
	 * direction) corresponding to the next step.
	 * @param x_robot the X coordinate of the current robot position.
	 * @param y_robot the Y coordinate of the current robot position.
	 * @param r_robot the current robot direction in rad, r = 0 means the direction is the x-axis, r follows 
	 * the trigonometric direction.
	 * @param landmarks the set of all known landmarks after being processed.
	 * @throws Exception thrown if it unable to find a next destination, thrown from DecisionMaker object.
	 */
	public void getNextPulses(double x_robot, double y_robot, double r_robot, Landmarks landmarks) throws Exception{
		if (this.scenario == 1){		//Scenario 1 : "Follow a trajectory"
			//Get index of next destination landmark
			this.index_current_dest = this.decider.getNextIndex(x_robot,y_robot,landmarks.getLandmark(this.index_current_dest),this.index_current_dest);
			this.destinationCoord[0] = landmarks.getLandmark(this.index_current_dest)[1];
			this.destinationCoord[1] = landmarks.getLandmark(this.index_current_dest)[2];
			
			//Calculate the information to reach this destination
			this.calculator.calculate_move_info(x_robot,y_robot,r_robot,this.destinationCoord);
			
		} else {						//Scenario 2&3 : "Avoid the sensors"
			//Get the next destination coordinates
			this.destinationCoord = this.decider.getNextPoint(landmarks, x_robot, y_robot,r_robot);
			//Calculate the information to reach this destination
	        this.calculator.calculate_move_info(x_robot,y_robot,r_robot,this.destinationCoord);
	       
		}
		
		//Put results in variables
		this.pulse_dist = this.calculator.getPulse_dist();
		this.pulse_rot = this.calculator.getPulse_rot();
		this.trigo = this.calculator.getTrigo();
	}

}
